# AprilCalib log 4
# CalibRig::mode=2d
# @ Mon Nov 25 12:08:16 2019

from numpy import array
U=array([[434.4640808105469, 384.5964660644531, 338.063232421875, 295.5713806152344, 257.34326171875, 381.5523986816406, 332.8322143554688, 288.4653625488281, 433.9382019042969, 326.7164611816406, 281.4390563964844, 431.3457946777344, 376.9552917480469, 234.42431640625, 274.4124450683594, 320.05712890625, 426.9634399414063, 371.0533142089844, 266.8536071777344, 227.4203643798828, 312.4828186035156, 420.02001953125, 363.7895202636719, 220.9977264404297, 259.8892517089844, 304.9496154785156, 355.6519470214844, 411.0414428710938, 215.4944458007813, 297.5776672363281, 253.8218536376953, 346.8219299316406, 400.4375915527344, 211.3534545898438, 248.3002014160156, 290.4208374023438, 388.7306823730469, 337.74462890625],
       [62.4888916015625, 63.99070739746094, 67.70628356933594, 72.19379425048828, 78.19879150390625, 109.2027816772461, 110.9877471923828, 113.9707946777344, 108.9248580932617, 159.5788421630859, 160.0745544433594, 161.9697418212891, 160.2123260498047, 207.3801879882813, 209.6023101806641, 212.3697662353516, 220.2707061767578, 215.7696228027344, 261.4444580078125, 255.4914703369141, 267.5050354003906, 281.2730102539063, 274.0280151367188, 304.1420593261719, 313.2212524414063, 322.5190124511719, 332.3667907714844, 342.2332458496094, 350.8785400390625, 375.5106811523438, 362.9067077636719, 387.9742126464844, 399.85888671875, 395.3750915527344, 409.5388793945313, 424.3113098144531, 452.0010375976563, 438.7295837402344]], dtype='float64');
Xw=array([[1499.504516601563, 1499.504516601563, 1499.504516601563, 1499.504516601563, 1499.504516601563, 1299.504516601563, 1299.504516601563, 1299.504516601563, 1299.504516601563, 1099.504516601563, 1099.504516601563, 1099.504516601563, 1099.504516601563, 899.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063, 899.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 699.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 499.5044555664063, 299.5044555664063, 299.5044555664063, 299.5044555664063, 299.5044555664063, 299.5044555664063, 99.50446319580078, 99.50446319580078, 99.50446319580078, 99.50446319580078, 99.50446319580078],
       [899.5044555664063, 699.5044555664063, 499.5044555664063, 299.5044555664063, 99.50446319580078, 699.5044555664063, 499.5044555664063, 299.5044555664063, 899.5044555664063, 499.5044555664063, 299.5044555664063, 899.5044555664063, 699.5044555664063, 99.50446319580078, 299.5044555664063, 499.5044555664063, 899.5044555664063, 699.5044555664063, 299.5044555664063, 99.50446319580078, 499.5044555664063, 899.5044555664063, 699.5044555664063, 99.50446319580078, 299.5044555664063, 499.5044555664063, 699.5044555664063, 899.5044555664063, 99.50446319580078, 499.5044555664063, 299.5044555664063, 699.5044555664063, 899.5044555664063, 99.50446319580078, 299.5044555664063, 499.5044555664063, 899.5044555664063, 699.5044555664063],
       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype='float64');
# After LM:
K=array([[420.1093438622507, 0, 314.3827721279553],
       [0, 418.7722961168358, 252.5955934923953],
       [0, 0, 1]], dtype='float64');
distCoeffs=array([-0.3753308298253078,
       0.1340535535006256,
       -0.0003604317032652451,
       -0.01046347797048779,
       0], dtype='float64');
CovK=array([[0.1930576651522896, 0.1884469478537592, -0.04189154343225485, -0.03895987085048608, -0.0004395457388068034, 0.0005416044750473184, 1.975803481444812e-05, 2.619468446661566e-05, -0.0001064845722816107],
       [0.1884469478537744, 0.1849972732981688, -0.03980577351199365, -0.03593269305720809, -0.0004276532586425032, 0.0005223723064254345, 1.850088906531881e-05, 2.534833836779378e-05, -0.0001006158655334841],
       [-0.04189154343206736, -0.03980577351180159, 0.04309756208392145, 0.01222513215536771, 5.54827951445209e-05, 4.889734071761066e-05, -3.58016004707152e-06, -1.787358075372116e-05, -0.0001836997339359339],
       [-0.03895987085041602, -0.03593269305714021, 0.01222513215533715, 0.04203489628477029, 7.527866393958237e-05, -6.038903079285451e-05, -1.623978428087428e-05, -7.976774410791923e-06, -6.322041896468838e-05],
       [-0.0004395457388067608, -0.0004276532586424254, 5.548279514472086e-05, 7.52786639397043e-05, 2.858022861544603e-06, -8.787766993661538e-06, -4.088644606494799e-08, -3.197176522991574e-08, 8.372029399492935e-06],
       [0.0005416044750471782, 0.0005223723064251874, 4.889734071813946e-05, -6.038903079291469e-05, -8.787766993661546e-06, 3.43211019593304e-05, 5.366964593304103e-08, -7.324469951620764e-08, -3.691728603262344e-05],
       [1.975803481443014e-05, 1.850088906530073e-05, -3.580160047068196e-06, -1.623978428087638e-05, -4.088644606490157e-08, 5.36696459329356e-08, 9.790760171918002e-09, 3.04792670268661e-09, 5.554063345834619e-09],
       [2.619468446656387e-05, 2.534833836773969e-05, -1.787358075374285e-05, -7.976774410812137e-06, -3.197176522984834e-08, -7.324469951612158e-08, 3.047926702690515e-09, 1.241628754207848e-08, 1.799687305747657e-07],
       [-0.0001064845722817061, -0.0001006158655334479, -0.0001836997339369671, -6.322041896477916e-05, 8.372029399493542e-06, -3.691728603262451e-05, 5.554063345736661e-09, 1.799687305749718e-07, 4.238761998904464e-05]], dtype='float64');
# rms=0.3107
r0=array([0.1542586849928198,
       0.1645086787089747,
       -1.951340399035287], dtype='float64');
t0=array([-627.8637664926536,
       701.9698988409309,
       1948.338267567272], dtype='float64');
Covr0=array([[4.677460045520303e-07, -3.750340052910537e-08, 2.86882645648585e-08],
       [-3.75034005294501e-08, 3.556255928709973e-07, 3.853888341444542e-08],
       [2.868826456484882e-08, 3.853888341444284e-08, 1.412555699115873e-08]], dtype='float64');
Covt0=array([[0.9339718287849341, 0.2784052508160192, 0.8915971052960254],
       [0.2784052508154103, 0.863545779841321, 0.4744133834749026],
       [0.8915971053003466, 0.4744133834767071, 3.684805189722633]], dtype='float64');
r1=array([-0.1647775533794807,
       0.07461798551992976,
       -0.03285063813163618], dtype='float64');
t1=array([-632.3880288021397,
       -586.0033255781805,
       1764.452062782287], dtype='float64');
Covr1=array([[2.323547817756425e-07, -2.842702683663187e-08, -1.103640657752277e-08],
       [-2.842702683650142e-08, 1.883903413213907e-07, 1.940392437114768e-08],
       [-1.103640657750545e-08, 1.94039243711354e-08, 5.663166441810359e-09]], dtype='float64');
Covt1=array([[0.7485315120609086, 0.2016191446962024, 0.8724804752454252],
       [0.2016191446956741, 0.7111780143875481, 0.7816973322529667],
       [0.8724804752484475, 0.7816973322540215, 3.484231055275638]], dtype='float64');
r2=array([-0.1115498475147438,
       -0.0546245168668773,
       0.6872187120126086], dtype='float64');
t2=array([-127.2493482535533,
       -1141.79628135693,
       1614.466163846507], dtype='float64');
Covr2=array([[2.383140839638492e-07, 6.941017903924457e-09, -9.259696549407456e-09],
       [6.941017903975733e-09, 2.060447679283698e-07, 2.417109481903547e-08],
       [-9.259696549416662e-09, 2.417109481902988e-08, 6.798112362329319e-09]], dtype='float64');
Covt2=array([[0.668897983739225, 0.1823724476880478, 0.6888900731206202],
       [0.1823724476876138, 0.626902698439703, 0.7920680536026273],
       [0.6888900731232447, 0.7920680536035878, 3.120091030365458]], dtype='float64');
r3=array([-0.2745237377272742,
       0.001610247213031174,
       0.06250323429182487], dtype='float64');
t3=array([-726.74749490034,
       -630.1299052777136,
       1723.458980359185], dtype='float64');
Covr3=array([[2.448935089008298e-07, -1.882002722659311e-08, -1.153089857016434e-08],
       [-1.88200272264276e-08, 1.694132346282394e-07, 2.459115800989781e-08],
       [-1.153089857016707e-08, 2.459115800991945e-08, 7.384526342446874e-09]], dtype='float64');
Covt3=array([[0.702709748113524, 0.193440154693214, 0.834918803346826],
       [0.1934401546927319, 0.6864267787311856, 0.7918945534744583],
       [0.8349188033494666, 0.791894553475471, 3.297971710517851]], dtype='float64');
r4=array([-0.4454848954075545,
       0.331564354474043,
       -1.414187481833552], dtype='float64');
t4=array([-534.2430073834473,
       697.9653094430223,
       1713.75101844813], dtype='float64');
Covr4=array([[3.709322744187386e-07, -7.805933215108385e-08, -1.141477048262559e-09],
       [-7.805933215125224e-08, 1.696531272860126e-07, 3.91047319342639e-08],
       [-1.141477048305961e-09, 3.910473193427946e-08, 1.45043913566126e-08]], dtype='float64');
Covt4=array([[0.7342294662180385, 0.2335776918388278, 0.7660929251999999],
       [0.2335776918383027, 0.6852113628822054, 0.3968674666823503],
       [0.7660929252029851, 0.3968674666835196, 2.458539305471377]], dtype='float64');
